/****************************************************************
 * Fulguro
 * Copyright (C) 2004 Christophe Clienti
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 ***************************************************************/


#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <flgrCoreMalloc.h>
#include <flgrCoreDispatch.h>

#ifdef __SSE2__
#include <emmintrin.h>
#else
#if defined(__SSE__) && defined(__MMX__)
#include <mmintrin.h>
#include <xmmintrin.h>
#endif
#endif

#include <flgrCoreTranspose.h>
#include "flgrMorphoBaseSegmentFast.h"


void flgr2d_erode_segment_vertical_fast_fgUINT8(FLGR_Data2D *imgdest, FLGR_Data2D *imgsrc, int sesize_y) {
#ifdef __SSE2__
  int i,j,k,m=0,n=0,p=0;
  int se2=sesize_y/2;
  int kpad;
  int rowsize_y=imgdest->size_y;
  int finalPass=rowsize_y%sesize_y;
  int nbbloc = rowsize_y/sesize_y;
  fgUINT8 **rowin;
  fgUINT8 **rowout;
  __m128i *bufg;
  __m128i *bufh;
  __m128i tmpg,tmph;

  FLGR_DECLARE_FUNCTION;

  bufg  = (__m128i *) flgr_malloc_align(sizeof(__m128i)*(rowsize_y+16),16);
  bufh  = (__m128i *) flgr_malloc_align(sizeof(__m128i)*(rowsize_y+16),16);

  rowin = (fgUINT8**) malloc(sizeof(fgUINT8*)*(imgsrc->size_y+16));
  rowout = (fgUINT8**) malloc(sizeof(fgUINT8*)*(rowsize_y+16));

  for(i=0 ; i<(rowsize_y+16) ; i++) {
    rowin[i] = (fgUINT8*) imgsrc->array[i];
    rowout[i] = (fgUINT8*) imgdest->array[i];
  }


  for(j=0 ; j<imgdest->size_x ; j+=16) {

    m=0;
    n=nbbloc*sesize_y-1;

    for(k=0;k<nbbloc;k++) {

      tmpg = bufg[m]=*((__m128i*) (rowin[m]+j));
      tmph = bufh[n]=*((__m128i*) (rowin[n]+j));
      m++;n--;

      for(i=1;i<sesize_y;i++,m++,n--) {
	bufg[m] = tmpg = _mm_min_epu8(*((__m128i*) (rowin[m]+j)),tmpg);
	bufh[n] = tmph = _mm_min_epu8(*((__m128i*) (rowin[n]+j)),tmph);
      }

    }

    n=rowsize_y-1;
    tmpg = bufg[m] = *((__m128i*) (rowin[m]+j));
    tmph = bufh[n] = *((__m128i*) (rowin[n]+j));
    m++;n--;

    for(k=1 ; k<finalPass ; k++,m++,n--) {
	bufg[m] = tmpg = _mm_min_epu8(*((__m128i*) (rowin[m]+j)),tmpg);
	bufh[n] = tmph = _mm_min_epu8(*((__m128i*) (rowin[n]+j)),tmph);
    }

    n=se2;p=0;m=0;
    for( i=0; i<se2 ; i++) {
      *((__m128i*) (rowout[m++]+j)) = bufg[n++];
    }
    for( i=se2; i<rowsize_y-se2 ;i++) {
      *((__m128i*) (rowout[m]+j)) = _mm_min_epu8(bufg[n], bufh[p]);
      n++;p++;m++;
    }
    n--;
    kpad = finalPass;
    kpad = (kpad==0) ? 0 : sesize_y-kpad;
    for( i=0 ; (i<kpad) && (m<rowsize_y) ; i++) {
      *((__m128i*) (rowout[m]+j)) = _mm_min_epu8(bufg[n], bufh[p]);
      m++;p++;
    }
    for( i=m ; i<rowsize_y ; i++) *((__m128i*) (rowout[i]+j)) = bufh[p++];

  }

  flgr_free_align(bufg);
  flgr_free_align(bufh);
  free(rowin);
  free(rowout);

  return;


#else
#if defined(__SSE__) && defined(__MMX__)
  int i,j,k,m=0,n=0,p=0;
  int kpad;
  int se2=sesize_y/2;
  int rowsize_y=imgdest->size_y;
  int finalPass=rowsize_y%sesize_y;
  int nbbloc = rowsize_y/sesize_y;
  fgUINT8 **rowin;
  fgUINT8 **rowout;
  __m64 *bufg;
  __m64 *bufh;
  __m64 tmpg,tmph;

  FLGR_DECLARE_FUNCTION;

  bufg  = (__m64 *) flgr_malloc_align(sizeof(__m64)*(rowsize_y+16),16);
  bufh  = (__m64 *) flgr_malloc_align(sizeof(__m64)*(rowsize_y+16),16);
  
  rowin = (fgUINT8**) malloc(sizeof(fgUINT8*)*(imgsrc->size_y+16));
  rowout = (fgUINT8**) malloc(sizeof(fgUINT8*)*(rowsize_y+16));

  for(i=0 ; i<(rowsize_y+16) ; i++) {
    rowin[i] = (fgUINT8*) imgsrc->array[i];
    rowout[i] = (fgUINT8*) imgdest->array[i];
  }

  _mm_empty();


  for(j=0 ; j<imgdest->size_x ; j+=8) {


    m=0;
    n=nbbloc*sesize_y-1;

    for(k=0;k<nbbloc;k++) {

      tmpg = bufg[m]=*((__m64*) (rowin[m]+j));
      tmph = bufh[n]=*((__m64*) (rowin[n]+j));
      m++;n--;

      for(i=1;i<sesize_y;i++,m++,n--) {
	bufg[m] = tmpg = _mm_min_pu8(*((__m64*) (rowin[m]+j)),tmpg);
	bufh[n] = tmph = _mm_min_pu8(*((__m64*) (rowin[n]+j)),tmph);	
      }
      
    }

    n=rowsize_y-1;
    tmpg = bufg[m] = *((__m64*) (rowin[m]+j));
    tmph = bufh[n] = *((__m64*) (rowin[n]+j));
    m++;n--;

    for(k=1 ; k<finalPass ; k++,m++,n--) {
	bufg[m] = tmpg = _mm_min_pu8(*((__m64*) (rowin[m]+j)),tmpg);
	bufh[n] = tmph = _mm_min_pu8(*((__m64*) (rowin[n]+j)),tmph);      
    }

    n=se2;p=0;m=0;
    for( i=0; i<se2 ; i++) {
      *((__m64*) (rowout[m++]+j)) = bufg[n++];
    }
    for( i=se2; i<rowsize_y-se2 ;i++) {
      *((__m64*) (rowout[m]+j)) = _mm_min_pu8(bufg[n], bufh[p]);
      n++;p++;m++;
    }
    n--;
    kpad = finalPass;
    kpad = (kpad==0) ? 0 : sesize_y-kpad;
    for( i=0 ; (i<kpad) && (m<rowsize_y) ; i++) {
      *((__m64*) (rowout[m]+j)) = _mm_min_pu8(bufg[n], bufh[p]);
      m++;p++;
    }
    for( i=m ; i<rowsize_y ; i++) *((__m64*) (rowout[i]+j)) = bufh[p++]; 

  }  

  flgr_free_align(bufg);
  flgr_free_align(bufh);
  free(rowin);
  free(rowout);

  _mm_empty();
  return;

 #else
   POST_ERROR("MMX/SSE/SSE2 Instruction not supported!\n");
 #endif
#endif
}



void flgr2d_dilate_segment_vertical_fast_fgUINT8(FLGR_Data2D *imgdest, FLGR_Data2D *imgsrc, int sesize_y) {
#ifdef __SSE2__
  int i,j,k,m=0,n=0,p=0;
  int se2=sesize_y/2;
  int kpad;
  int rowsize_y=imgdest->size_y;
  int finalPass=rowsize_y%sesize_y;
  int nbbloc = rowsize_y/sesize_y;
  fgUINT8 **rowin;
  fgUINT8 **rowout;
  __m128i *bufg;
  __m128i *bufh;
  __m128i tmpg,tmph;

  FLGR_DECLARE_FUNCTION;

  bufg  = (__m128i *) flgr_malloc_align(sizeof(__m128i)*(rowsize_y+16),16);
  bufh  = (__m128i *) flgr_malloc_align(sizeof(__m128i)*(rowsize_y+16),16);

  rowin = (fgUINT8**) malloc(sizeof(fgUINT8*)*(imgsrc->size_y+16));
  rowout = (fgUINT8**) malloc(sizeof(fgUINT8*)*(rowsize_y+16));

  for(i=0 ; i<(rowsize_y+16) ; i++) {
    rowin[i] = (fgUINT8*) imgsrc->array[i];
    rowout[i] = (fgUINT8*) imgdest->array[i];
  }



  for(j=0 ; j<imgdest->size_x ; j+=16) {

    m=0;
    n=nbbloc*sesize_y-1;

    for(k=0;k<nbbloc;k++) {

      tmpg = bufg[m]=*((__m128i*) (rowin[m]+j));
      tmph = bufh[n]=*((__m128i*) (rowin[n]+j));
      m++;n--;

      for(i=1;i<sesize_y;i++,m++,n--) {
	bufg[m] = tmpg = _mm_max_epu8(*((__m128i*) (rowin[m]+j)),tmpg);
	bufh[n] = tmph = _mm_max_epu8(*((__m128i*) (rowin[n]+j)),tmph);
      }

    }

    n=rowsize_y-1;
    tmpg = bufg[m] = *((__m128i*) (rowin[m]+j));
    tmph = bufh[n] = *((__m128i*) (rowin[n]+j));
    m++;n--;

    for(k=1 ; k<finalPass ; k++,m++,n--) {
	bufg[m] = tmpg = _mm_max_epu8(*((__m128i*) (rowin[m]+j)),tmpg);
	bufh[n] = tmph = _mm_max_epu8(*((__m128i*) (rowin[n]+j)),tmph);
    }

    n=se2;p=0;m=0;
    for( i=0; i<se2 ; i++) {
      *((__m128i*) (rowout[m++]+j)) = bufg[n++];
    }
    for( i=se2; i<rowsize_y-se2 ;i++) {
      *((__m128i*) (rowout[m]+j)) = _mm_max_epu8(bufg[n], bufh[p]);
      n++;p++;m++;
    }
    n--;
    kpad = finalPass;
    kpad = (kpad==0) ? 0 : sesize_y-kpad;
    for( i=0 ; (i<kpad) && (m<rowsize_y) ; i++) {
      *((__m128i*) (rowout[m]+j)) = _mm_max_epu8(bufg[n], bufh[p]);
      m++;p++;
    }
    for( i=m ; i<rowsize_y ; i++) *((__m128i*) (rowout[i]+j)) = bufh[p++];

  }

  flgr_free_align(bufg);
  flgr_free_align(bufh);
  free(rowin);
  free(rowout);

  return;

#else
#if defined(__SSE__) && defined(__MMX__)
  int i,j,k,m=0,n=0,p=0;
  int kpad;
  int se2=sesize_y/2;
  int rowsize_y=imgdest->size_y;
  int finalPass=rowsize_y%sesize_y;
  int nbbloc = rowsize_y/sesize_y;
  fgUINT8 **rowin;
  fgUINT8 **rowout;
  __m64 *bufg;
  __m64 *bufh;
  __m64 tmpg,tmph;

  FLGR_DECLARE_FUNCTION;

  bufg  = (__m64 *) flgr_malloc_align(sizeof(__m64)*(rowsize_y+16),16);
  bufh  = (__m64 *) flgr_malloc_align(sizeof(__m64)*(rowsize_y+16),16);
  
  rowin = (fgUINT8**) malloc(sizeof(fgUINT8*)*(imgsrc->size_y+16));
  rowout = (fgUINT8**) malloc(sizeof(fgUINT8*)*(rowsize_y+16));

  for(i=0 ; i<(rowsize_y+16) ; i++) {
    rowin[i] = (fgUINT8*) imgsrc->array[i];
    rowout[i] = (fgUINT8*) imgdest->array[i];
  }

  _mm_empty();


  for(j=0 ; j<imgdest->size_x ; j+=8) {


    m=0;
    n=nbbloc*sesize_y-1;

    for(k=0;k<nbbloc;k++) {

      tmpg = bufg[m]=*((__m64*) (rowin[m]+j));
      tmph = bufh[n]=*((__m64*) (rowin[n]+j));
      m++;n--;

      for(i=1;i<sesize_y;i++,m++,n--) {
	bufg[m] = tmpg = _mm_max_pu8(*((__m64*) (rowin[m]+j)),tmpg);
	bufh[n] = tmph = _mm_max_pu8(*((__m64*) (rowin[n]+j)),tmph);	
      }
      
    }

    n=rowsize_y-1;
    tmpg = bufg[m] = *((__m64*) (rowin[m]+j));
    tmph = bufh[n] = *((__m64*) (rowin[n]+j));
    m++;n--;

    for(k=1 ; k<finalPass ; k++,m++,n--) {
	bufg[m] = tmpg = _mm_max_pu8(*((__m64*) (rowin[m]+j)),tmpg);
	bufh[n] = tmph = _mm_max_pu8(*((__m64*) (rowin[n]+j)),tmph);      
    }

    n=se2;p=0;m=0;
    for( i=0; i<se2 ; i++) {
      *((__m64*) (rowout[m++]+j)) = bufg[n++];
    }
    for( i=se2; i<rowsize_y-se2 ;i++) {
      *((__m64*) (rowout[m]+j)) = _mm_max_pu8(bufg[n], bufh[p]);
      n++;p++;m++;
    }
    n--;
    kpad = finalPass;
    kpad = (kpad==0) ? 0 : sesize_y-kpad;
    for( i=0 ; (i<kpad) && (m<rowsize_y) ; i++) {
      *((__m64*) (rowout[m]+j)) = _mm_max_pu8(bufg[n], bufh[p]);
      m++;p++;
    }
    for( i=m ; i<rowsize_y ; i++) *((__m64*) (rowout[i]+j)) = bufh[p++]; 

  }  

  flgr_free_align(bufg);
  flgr_free_align(bufh);
  free(rowin);
  free(rowout);

  _mm_empty();
  return ;

#else
  POST_ERROR("MMX/SSE/SSE2 Instruction not supported!\n");
#endif
#endif
}



void flgr2d_erode_segment_horizontal_fast_fgUINT8(FLGR_Data2D *imgdest, FLGR_Data2D *imgsrc, int sesize_x) {
  FLGR_Data2D *tmpdest,*tmpsrc;

  FLGR_DECLARE_FUNCTION;

  tmpdest = flgr2d_create_pixmap(imgdest->size_x,imgdest->size_y,imgsrc->spp,imgdest->type);
  tmpsrc = flgr2d_create_pixmap(imgdest->size_x,imgdest->size_y,imgsrc->spp,imgdest->type);

  flgr2d_transpose_fgUINT8(tmpsrc,imgsrc);
  flgr2d_erode_segment_vertical_fast_fgUINT8(tmpdest,tmpsrc,sesize_x);
  flgr2d_transpose_fgUINT8(imgdest,tmpdest);

  flgr2d_destroy(tmpdest);
  flgr2d_destroy(tmpsrc);

  return ;
}


void flgr2d_dilate_segment_horizontal_fast_fgUINT8(FLGR_Data2D *imgdest, FLGR_Data2D *imgsrc, int sesize_x) {
  FLGR_Data2D *tmpdest,*tmpsrc;

  FLGR_DECLARE_FUNCTION;

  tmpdest = flgr2d_create_pixmap(imgdest->size_x,imgdest->size_y,imgsrc->spp,imgdest->type);
  tmpsrc = flgr2d_create_pixmap(imgdest->size_x,imgdest->size_y,imgsrc->spp,imgdest->type);

  flgr2d_transpose_fgUINT8(tmpsrc,imgsrc);
  flgr2d_dilate_segment_vertical_fast_fgUINT8(tmpdest,tmpsrc,sesize_x);
  flgr2d_transpose_fgUINT8(imgdest,tmpdest);

  flgr2d_destroy(tmpdest);
  flgr2d_destroy(tmpsrc);

  return ;

}
